#include "lionav.h"

namespace Lio {

void getNextR(MATRIX3 R, double w[3], double time, MATRIX3 out)
{
	if(!out)out=R;	
	MATRIX3 tmp;
	tmp[0][0]=1;
	tmp[0][1]=-w[ROT_R]*time;
	tmp[0][2]=w[ROT_Q]*time;

	tmp[1][0]=w[ROT_R]*time;
	tmp[1][1]=1;
	tmp[1][2]=-w[ROT_P]*time;

	tmp[2][0]=-w[ROT_Q]*time;
	tmp[2][1]=w[ROT_P]*time;
	tmp[2][2]=1;
	
	mulMatrix(R,tmp);
}
void EulerToRbn(double roll, double pitch, double yaw, MATRIX3 Rbn)
{
	double cyaw=cos(yaw);
	double cpitch=cos(pitch);
	double croll=cos(roll);
	double syaw=sin(yaw);
	double spitch=sin(pitch);
	double sroll=sin(roll);

	Rbn[0][0]=cpitch*cyaw;
	Rbn[1][0]=cpitch*syaw;
	Rbn[2][0]=-spitch;

	Rbn[0][1]=spitch*sroll*cyaw-syaw*croll;
	Rbn[1][1]=syaw*spitch*sroll+cyaw*croll;
	Rbn[2][1]=cpitch*sroll;

	Rbn[0][2]=spitch*croll*cyaw+syaw*sroll;
	Rbn[1][2]=syaw*spitch*croll-cyaw*sroll;
	Rbn[2][2]=cpitch*croll;
}
void GeoToRen(double lat, double lon, MATRIX3 Ren)
{
	double clat=cos(lat);
	double clon=cos(lon);
	double slat=sin(lat);
	double slon=sin(lon);

	Ren[0][0]=-slat*clon;
	Ren[1][0]=-slat*slon;
	Ren[2][0]=clat;

	Ren[0][1]=-slon;
	Ren[1][1]=clon;
	Ren[2][1]=0;

	Ren[0][2]=-clat*clon;
	Ren[1][2]=-clat*slon;
	Ren[2][2]=-slat;
}
void normalR(MATRIX3 R, MATRIX3 out)
{
	if(!out)out=R;
	double tmp;
	tmp=sqrt(R[0][0]*R[0][0]+R[1][0]*R[1][0]+R[2][0]*R[2][0]);
	out[0][0]=R[0][0]/tmp;
	out[1][0]=R[1][0]/tmp;
	out[2][0]=R[2][0]/tmp;
	tmp=sqrt(R[0][1]*R[0][1]+R[1][1]*R[1][1]+R[2][1]*R[2][1]);
	out[0][1]=R[0][1]/tmp;
	out[1][1]=R[1][1]/tmp;
	out[2][1]=R[2][1]/tmp;
	tmp=sqrt(R[0][2]*R[0][2]+R[1][2]*R[1][2]+R[2][2]*R[2][2]);
	out[0][2]=R[0][2]/tmp;
	out[1][2]=R[1][2]/tmp;
	out[2][2]=R[2][2]/tmp;
}
void RbnToEuler(CMATRIX3 Rbn, double euler[3])
{
	euler[ROT_R]=atan2(Rbn[1][0],Rbn[0][0]);
	euler[ROT_Q]=asin(-Rbn[2][0]);
	euler[ROT_P]=atan2(Rbn[2][1],Rbn[2][2]);
}
void EulerToQuat(const double euler[3], QUAT q)
{
	double cyaw2=cos(euler[ROT_R]/2);
	double cpitch2=cos(euler[ROT_Q]/2);
	double croll2=cos(euler[ROT_P]/2);
	double syaw2=sin(euler[ROT_R]/2);
	double spitch2=sin(euler[ROT_Q]/2);
	double sroll2=sin(euler[ROT_P]/2);

	q[0]=cyaw2*cpitch2*croll2+syaw2*spitch2*sroll2;
	q[1]=cyaw2*cpitch2*sroll2-syaw2*spitch2*croll2;
	q[2]=cyaw2*spitch2*croll2+syaw2*cpitch2*sroll2;
	q[3]=syaw2*cpitch2*croll2-cyaw2*spitch2*sroll2;
}
void quatToEuler(CQUAT q, double euler[3])
{
	euler[ROT_R]=atan2(	2*(q[1]*q[2]+q[0]*q[3]),
							q[0]*q[0]+q[1]*q[1]-q[2]*q[2]-q[3]*q[3]);
	euler[ROT_Q]=asin(-2*(q[1]*q[3]-q[0]*q[2]));
	euler[ROT_P]=atan2(	2*(q[2]*q[3]+q[0]*q[1]),
							q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3]);
}
void quatToRbn(CQUAT q, MATRIX3 Rbn)
{
	Rbn[0][0]=q[0]*q[0]+q[1]*q[1]-q[2]*q[2]-q[3]*q[3];
	Rbn[0][1]=2*(q[1]*q[2]-q[0]*q[3]);
	Rbn[0][2]=2*(q[1]*q[3]+q[0]*q[2]);
	
	Rbn[1][0]=2*(q[1]*q[2]+q[0]*q[3]);
	Rbn[1][1]=q[0]*q[0]-q[1]*q[1]+q[2]*q[2]-q[3]*q[3];
	Rbn[1][2]=2*(q[2]*q[3]-q[0]*q[1]);
	
	Rbn[2][0]=2*(q[1]*q[3]-q[0]*q[2]);
	Rbn[2][1]=2*(q[2]*q[3]+q[0]*q[1]);
	Rbn[2][2]=q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
}
void RbnToQuat(CMATRIX3 Rbn, QUAT q)
{
	double trace,tmp;
	trace = Rbn[0][0] + Rbn[1][1] + Rbn[2][2];
	if(trace >= 0)
	{
		tmp = sqrt(trace + 1);
		q[0] = 0.5 * tmp;
		tmp = 0.5 / tmp;
		q[1] = (Rbn[2][1] - Rbn[1][2]) * tmp;
		q[2] = (Rbn[0][2] - Rbn[2][0]) * tmp;
		q[3] = (Rbn[1][0] - Rbn[0][1]) * tmp;
	}
	else
	{
		if(Rbn[2][2] > Rbn[0][0])
		{
			tmp = sqrt((Rbn[2][2] - (Rbn[0][0] + Rbn[1][1])) + 1);
			q[3] = 0.5 * tmp;
			tmp = 0.5 / tmp;
			q[1] = (Rbn[2][0] + Rbn[0][2]) * tmp;
			q[2] = (Rbn[1][2] + Rbn[2][1]) * tmp;
			q[0] = (Rbn[1][0] - Rbn[0][1]) * tmp;
		}
		else if(Rbn[1][1] > Rbn[0][0])
		{
			tmp = sqrt((Rbn[1][1] - (Rbn[2][2] + Rbn[0][0])) + 1);
			q[2] = 0.5 * tmp;
			tmp = 0.5 / tmp;
			q[3] = (Rbn[1][2] + Rbn[2][1]) * tmp;
			q[1] = (Rbn[0][1] + Rbn[1][0]) * tmp;
			q[0] = (Rbn[0][2] - Rbn[2][0]) * tmp;
		}
		else
		{
			tmp = sqrt((Rbn[0][0] - (Rbn[1][1] + Rbn[2][2])) + 1);
			q[1] = 0.5 * tmp;
			tmp = 0.5 / tmp;
			q[2] = (Rbn[0][1] + Rbn[1][0]) * tmp;
			q[3] = (Rbn[2][0] + Rbn[0][2]) * tmp;
			q[0] = (Rbn[2][1] - Rbn[1][2]) * tmp;
		}
	}
}
void getNextQuat(double *q, const double *w, double time, double *out, BOOL normalize)
{
	if(!out)out=q;
	out[0]=q[0]-0.5*( w[ROT_P]*q[1]+w[ROT_Q]*q[2]+w[ROT_R]*q[3])*time;
	out[1]=q[1]+0.5*( w[ROT_P]*q[0]-w[ROT_Q]*q[3]+w[ROT_R]*q[2])*time;
	out[2]=q[2]+0.5*( w[ROT_P]*q[3]+w[ROT_Q]*q[0]-w[ROT_R]*q[1])*time;
	out[3]=q[3]+0.5*(-w[ROT_P]*q[2]+w[ROT_Q]*q[1]+w[ROT_R]*q[0])*time;
	if(normalize)normQuat(out);
}
void LLAtoECEF(const double *lla, double *ecef)
{
	double clat=cos(lla[LLA_LAT]);
	double slat=sin(lla[LLA_LAT]);
	double Rt=A_EARTH/sqrt(1-pow2(E_EARTH*slat));
	ecef[0]=(Rt+lla[LLA_ALT])*clat*cos(lla[LLA_LON]);
	ecef[1]=(Rt+lla[LLA_ALT])*clat*sin(lla[LLA_LON]);
	ecef[2]=(Rt*(1-E2_EARTH)+lla[LLA_ALT])*slat;
}
void ECEFtoLLA(const double *ecef, double *lla, int iterCount)
{
	lla[LLA_LON]=atan2(ecef[AXE_Y],ecef[AXE_X]);
	double p=sqrt(pow2(ecef[AXE_X])+pow2(ecef[AXE_Y])); // p=sqrt(x^2+y^2)
	double beta,slat,clat;
	beta=atan2(ecef[AXE_Z],K_EARTH*p);
	for(;;)
	{
		lla[LLA_LAT]=atan2(	ecef[AXE_Z]+E2_EARTH*A_EARTH*pow3(sin(beta))/K_EARTH,
							p-E2_EARTH*A_EARTH*pow3(cos(beta)));
		slat=sin(lla[LLA_LAT]);
		clat=cos(lla[LLA_LAT]);
		if(iterCount<=0)
			break;
        iterCount--;
		beta=atan2(K_EARTH*slat,clat);
	}
	if(slat<clat)
		lla[LLA_ALT]=p/clat-A_EARTH/sqrt(1-pow2(E_EARTH*slat));
	else
		lla[LLA_ALT]=ecef[AXE_Z]/slat-(1-E2_EARTH)*A_EARTH/sqrt(1-pow2(E_EARTH*slat));
}
void LLAtoGeoRef(const double* lla, double* map)
{
	map[AXE_X]=A_EARTH*(1-E2_EARTH)*wef(lla[LLA_LAT]);
	map[AXE_Y]=(A_EARTH*cos(lla[LLA_LAT])/sqrt(1-pow2(E_EARTH*sin(lla[LLA_LAT]))))*lla[LLA_LON];
	map[AXE_Z]=lla[LLA_ALT];
}
void GeoReftoLLA(const double* map, double* lla)
{
	lla[LLA_LAT]=invwef(map[AXE_X]/(A_EARTH*(1-E2_EARTH)));
	lla[LLA_LON]=(sqrt(1-E2_EARTH*pow2(sin(lla[LLA_LAT])))/(A_EARTH*cos(lla[LLA_LAT])))*map[AXE_Y];
	lla[LLA_ALT]=map[AXE_Z];
}
INS::INS():beta_a(0),beta_w(0),recurs(TRUE)
{
	A.setSize(STATE_SIZE,STATE_SIZE,TRUE);
	Q.setSize(STATE_SIZE,STATE_SIZE,TRUE);
	R.setSize(CORRECT_SIZE,CORRECT_SIZE,TRUE);
	dX.setSize(STATE_SIZE,1,TRUE);
	dXest.setSize(STATE_SIZE,1,TRUE);
	P.setSize(STATE_SIZE,STATE_SIZE,TRUE);
	Pest.setSize(STATE_SIZE,STATE_SIZE,TRUE);
	H.setSize(CORRECT_SIZE,STATE_SIZE,TRUE);
	H.elems[CORRECT_LAT][STATE_LAT]=1;
	H.elems[CORRECT_LON][STATE_LON]=1;
	H.elems[CORRECT_ALT][STATE_ALT]=1;
	Ht=H.trans();
	Z.setSize(CORRECT_SIZE,1,TRUE);
}
void INS::init(	const double *r0, const double *vn0, CQUAT q0,
				const double *input_accel, const double *input_gyro)
{
	initVector(r, r0);
	initVector(vn, vn0);
	initQuat(q, q0);
	time=0;
	dt=0;
	dX.clear();
	dXest.clear();
	P.clear();
	Pest.clear();
	estimate(0,input_accel,input_gyro);
}
void INS::setStateNoise(const double *dev_state, double beta_accel, double beta_gyro)
{
	if(!dev_state)
		ClearArray(dQ,STATE_SIZE);
	else for(int i=0;i<STATE_SIZE;i++)
		dQ[i]=pow2(dev_state[i]);
	beta_a=ABS(beta_accel);
	beta_w=ABS(beta_gyro);
}
void INS::setMeasureNoise(const Matrix &newR)
{
	R.copy(newR);
}
void INS::estimate(double sample, const double *input_accel, const double *input_gyro)
{
	double slat,clat,tmp,Wtmp[3];
	if(sample<0)
		return;
	// Set the new sample time and inputs
	dt=sample;
	initVector(ab, input_accel);
	initVector(Wib_b, input_gyro);

	// UPDATE STATE
	// Go to the next state, using the state derivates from the previous
	time+=dt;
	addVector(r_dot,r,dt,1);
	addVector(vn_dot,vn,dt,1);
	getNextQuat(q,Wnb_b,dt);
	// UPDATE ERROR
	// Go to the next dX, using the dX derivates from the previous
	// A - dX transfer matrix
	A.one();
	// A[0][1] = D^-1*dt
	A.elems[ 0][ 3]= dt/D11;
	A.elems[ 1][ 4]= dt/D22;
	A.elems[ 2][ 5]=-dt;
	// A[1][2] = S(fn)*dt
	A.elems[ 3][ 7]= dt*an[2];
	A.elems[ 3][ 8]=-dt*an[1];
	A.elems[ 4][ 6]=-dt*an[2];
	A.elems[ 4][ 8]= dt*an[0];
	A.elems[ 5][ 6]= dt*an[1];
	A.elems[ 5][ 7]=-dt*an[0];
	// A[1][3] = Rbn*dt
	A.elems[ 3][ 9]= dt*Rbn[0][0];
	A.elems[ 3][10]= dt*Rbn[0][1];
	A.elems[ 3][11]= dt*Rbn[0][2];
	A.elems[ 4][ 9]= dt*Rbn[1][0];
	A.elems[ 4][10]= dt*Rbn[1][1];
	A.elems[ 4][11]= dt*Rbn[1][2];
	A.elems[ 5][ 9]= dt*Rbn[2][0];
	A.elems[ 5][10]= dt*Rbn[2][1];
	A.elems[ 5][11]= dt*Rbn[2][2];
	// A[2][4] = -Rbn*dt
	A.elems[ 6][12]=-dt*Rbn[0][0];
	A.elems[ 6][13]=-dt*Rbn[0][1];
	A.elems[ 6][14]=-dt*Rbn[0][2];
	A.elems[ 7][12]=-dt*Rbn[1][0];
	A.elems[ 7][13]=-dt*Rbn[1][1];
	A.elems[ 7][14]=-dt*Rbn[1][2];
	A.elems[ 8][12]=-dt*Rbn[2][0];
	A.elems[ 8][13]=-dt*Rbn[2][1];
	A.elems[ 8][14]=-dt*Rbn[2][2];
	// A[3][3] = exp(-ba*dt) ~= I-bf*dt
	tmp=exp(-beta_a*dt);
	A.elems[ 9][ 9]=tmp;
	A.elems[10][10]=tmp;
	A.elems[11][11]=tmp;
	// A[4][4] = exp(-bw*dt) ~= I-bw*dt
	tmp=exp(-beta_w*dt);
	A.elems[12][12]=tmp;
	A.elems[13][13]=tmp;
	A.elems[14][14]=tmp;
	
	// Input Error matrix
	// Q[0][0]=Qr*dt
	tmp=dt*dt*dt*dt;
	Q.elems[ 0][ 0]=dQ[ 0]*tmp;
	Q.elems[ 1][ 1]=dQ[ 1]*tmp;
	Q.elems[ 2][ 2]=dQ[ 2]*tmp;
	// Q[1][1]=Qv*dt
	Q.elems[ 3][ 3]=dQ[ 3]*tmp;
	Q.elems[ 4][ 4]=dQ[ 4]*tmp;
	Q.elems[ 5][ 5]=dQ[ 5]*tmp;
	// Q[2][2]=Qq*dt
	Q.elems[ 6][ 6]=dQ[ 6]*tmp;
	Q.elems[ 7][ 7]=dQ[ 7]*tmp;
	Q.elems[ 8][ 8]=dQ[ 8]*tmp;
	// Q[3][3]=Qa*(1-e^(-2ba*dt))
	tmp=1-exp(-2*beta_a*dt);
	Q.elems[ 9][ 9]=dQ[ 9]*tmp;
	Q.elems[10][10]=dQ[10]*tmp;
	Q.elems[11][11]=dQ[11]*tmp;
	// Q[4][4]=Qa*(1-e^(-2ba*dt))
	tmp=1-exp(-2*beta_w*dt);
	Q.elems[12][12]=dQ[12]*tmp;
	Q.elems[13][13]=dQ[13]*tmp;
	Q.elems[14][14]=dQ[14]*tmp;
	// PREDICTION:
	if(recurs)
	{
		Pest=P=A*P*A.trans()+Q;
		dXest=dX=A*dX;
	}
	else
	{
		Pest=A*Pest*A.trans()+Q;
		dXest=A*dXest;
	}

	// UPDATE PARAMS
	// Calculate the new derivates for the next step
	quatToRbn(q,Rbn); // new navigation rotation
	transMatrix(Rbn,Rnb);
	// Curvature radiuses
	slat=sin(r[LLA_LAT]);
	clat=cos(r[LLA_LAT]);
	tmp=1-E2_EARTH*slat*slat;
	Rm=A_EARTH*(1-E2_EARTH)/tmp;
	Rt=A_EARTH;
	tmp=sqrt(tmp);
	Rm/=tmp;
	Rt/=tmp;
	D22=(Rt+r[LLA_ALT])*clat;
	D11=Rm+r[LLA_ALT];
	// Earth rotation
	Wie_n[0]=clat*W_EARTH;
	Wie_n[1]=0;
	Wie_n[2]=-slat*W_EARTH;
	// Navigation rotation
	Wen_n[0]= clat*vn[NED_E]/D22;
	Wen_n[1]=-vn[NED_N]/D11;
	Wen_n[2]=-slat*vn[NED_E]/D22;
	// Both Combined
	addVector(Wie_n,Wen_n,Win_n);
	mulVector(Rnb,Win_n,Win_b);

	// Full body rotation
	//initVector(Wnb_b,gyro);
	//addVector(Wnb_b,Win_b,Wib_b);
	addVector(Wib_b,Win_b,1,-1,Wnb_b);
	// Cartesian Earth position
	re[0]= D22*cos(r[LLA_LON]);
	re[1]= D22*sin(r[LLA_LON]);
	re[2]= (Rt*(1-E2_EARTH)+r[LLA_ALT])*slat;
	// Cartesian navigation position
	mapr[MAP_U]= Rt*clat*r[LLA_LON];
	mapr[MAP_V]= A_EARTH*(1-E2_EARTH)*wef(r[LLA_LAT]);
	ellipsoidMap(E_EARTH,re,map);

	// Body acceleration in navigation frame
	mulVector(Rbn,ab,an);
	// aCor=-(2Wie+Wen)xVn
	addVector(Wie_n,Wen_n,-2,-1,Wtmp);
	cross(Wtmp,vn,aInert);
	// -We^2(Rt+window.height)cos(r[LLA_LAT])
	aCen=-pow2(W_EARTH)*D22;
	aInert[0]+=aCen*slat;
	aInert[2]+=aCen*clat;
	// Navigation acceleration:
	addVector(aInert,an,vn_dot);
	vn_dot[2]+=G_EARTH;
	// Geodetic speed
	r_dot[LLA_LAT]= vn[NED_N]/D11;
	r_dot[LLA_LON]= vn[NED_E]/D22;
	r_dot[LLA_ALT]=-vn[NED_D];
}
BOOL INS::correct(const double measure[3])
{
	Z.elems[0][0]=r[0]-measure[0];
	Z.elems[1][0]=r[1]-measure[1];
	Z.elems[2][0]=r[2]-measure[2];
	// CORRECTION:
	Pz=H*Pest*H.trans()+R;
	try
	{
		K=Pest*H.trans()*Pz.inv();
		P=Pest-K*Pz*K.trans();
		dX=dXest+K*(Z-H*dXest);
	}
	catch(...)
	{
		return FALSE;
	}
	return TRUE;
}
void INS::transRotation(const double *Wnb_b, double *Wib_b)
{
	quatToRbn(q,Rbn);
	transMatrix(Rbn,Rnb);
	double slat=sin(r[LLA_LAT]);
	double clat=cos(r[LLA_LAT]);
	double tmp=1-E2_EARTH*slat*slat;
	Rm=A_EARTH*(1-E2_EARTH)/tmp;
	Rt=A_EARTH;
	tmp=sqrt(tmp);
	Rm/=tmp;
	Rt/=tmp;
	D22=(Rt+r[LLA_ALT])*clat;
	D11=Rm+r[LLA_ALT];
	Wie_n[0]=clat*W_EARTH;
	Wie_n[1]=0;
	Wie_n[2]=-slat*W_EARTH;
	Wen_n[0]= clat*vn[NED_E]/D22;
	Wen_n[1]=-vn[NED_N]/D11;
	Wen_n[2]=-slat*vn[NED_E]/D22;
	addVector(Wie_n,Wen_n,Win_n);
	mulVector(Rnb,Win_n,Win_b);
	addVector(Wnb_b,Win_b,Wib_b);
}

}; // namespace Lio